// Copyright 2006-2014 Coppelia Robotics GmbH. All rights reserved. 
// marc@coppeliarobotics.com
// www.coppeliarobotics.com
// 
// -------------------------------------------------------------------
// THIS FILE IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
// WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL
// AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS,
// DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR
// MISUSING THIS SOFTWARE.
// 
// You are free to use/modify/distribute this file for whatever purpose!
// -------------------------------------------------------------------
//
// This file was automatically created for V-REP release V3.1.2 on June 16th 2014

// Make sure to have the server side running in V-REP!
// Start the server from a child script with following command:
// simExtRemoteApiStart(portNumber) -- starts a remote API server service on the specified port
#define _CRT_SECURE_NO_WARNINGS
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <boost\asio.hpp>
#include <boost\algorithm\string.hpp>
#include <Message.h>
using namespace AgentsLanguage;
using namespace std;
using boost::asio::ip::tcp;

extern "C" {

#include "extApi.h"
	/*	#include "extApiCustom.h" if you wanna use custom remote API functions! */
}


void getmemory(unsigned char **p, int rX, int rY){
	*p = (unsigned char *)malloc(rX*rY * 3 * sizeof(unsigned char));
}

void printInSequence(unsigned char *bufferImage, int resolutionX, int resolutionY)  //print in sequence from 1 to X*Y*3
{
	int Kprint;
	int Kcount;
	Kcount = resolutionX*resolutionY * 3;
	for (Kprint = 0; Kprint != Kcount; Kprint++)
	{
		printf("unsigned bufferImage[%d]=%d\n", Kprint, (unsigned char)bufferImage[Kprint]); // convert to unsigned char and output
		printf("bufferImage[%d]=%d\n", Kprint, bufferImage[Kprint]);
	}
}


//----------------------------------------------


int main(int argc, char* argv[])
{
	char buffer[100];

	int portNb = 0;
	int redundantTipHandle;
	int rotationHandle;

	bool connected = false;

	if (argc >= 4)
	{
		portNb = atoi(argv[1]);
		redundantTipHandle = atoi(argv[2]);
		rotationHandle = atoi(argv[3]);
	}
	else
	{
		printf("Indicate following arguments: 'portNumber redundantTipHandle'!\n");
		extApi_sleepMs(5000);
		return 0;
	}


	try{
		boost::asio::io_service ioService;

		tcp::resolver resolver(ioService);
		tcp::resolver::query query(tcp::v4(), "localhost", "1313");
		tcp::resolver::iterator iterator = resolver.resolve(query);

		tcp::socket s(ioService);
		boost::asio::connect(s, iterator);
		connected = true;
	}
	catch (std::exception &e)
	{
		connected = false;
	}



	int clientID = simxStart((simxChar*)"127.0.0.1", portNb, true, true, 2000, 5);
	if (clientID != -1)
	{
		int cont = 0;
		int position_semaphore = 0;
		simxUChar redundantTipPosition = 0;

		simxInt consoleHandle;



		simxAuxiliaryConsoleOpen(clientID, "AuxConsoleRedundantManip", 10, 0x111111111111111, NULL, NULL, NULL, NULL, &consoleHandle, simx_opmode_oneshot_wait);
		//simxInt simxAuxiliaryConsolePrint(simxInt clientID,simxInt consoleHandle,const simxChar* txt,simxInt operationMode)
		simxAuxiliaryConsolePrint(clientID, consoleHandle, "Manipulator Test FABRO!\n", simx_opmode_oneshot_wait);

		while (simxGetConnectionId(clientID) != -1)
		{
			sprintf(buffer, "position_semaphore = %d   Cont = %d\n", position_semaphore, cont++);
			simxAuxiliaryConsolePrint(clientID, consoleHandle, buffer, simx_opmode_oneshot_wait);
			//simxSetJointTargetVelocity(clientID,redundantTipHandle,0.1,simx_opmode_oneshot);			
			if (cont >= 500)
			{
				cont = 0;
				if (position_semaphore == 1)
					position_semaphore = 0;
				else
					position_semaphore = 1;
			}
			//simxInt simxSetJointTargetVelocity(simxInt clientID,simxInt jointHandle,simxFloat targetVelocity,simxInt operationMode)
			simxSetJointTargetVelocity(clientID, redundantTipHandle, 0.1, simx_opmode_oneshot);
			simxSetJointTargetVelocity(clientID, rotationHandle, 0.1, simx_opmode_oneshot);

			if (position_semaphore == 0)
			{
				simxSetJointTargetPosition(clientID, redundantTipHandle, 3.1415 * 30.0 / 180.0, simx_opmode_oneshot);
				simxSetJointTargetPosition(clientID, rotationHandle, 3.1415 * 5.0 / 180.0, simx_opmode_oneshot);
			}
			else
			{
				simxSetJointTargetPosition(clientID, redundantTipHandle, -3.1415 * 5.0 / 180.0, simx_opmode_oneshot);
				simxSetJointTargetPosition(clientID, rotationHandle, -3.1415 / 4.0, simx_opmode_oneshot);
			}

			extApi_sleepMs(5);
		}
		simxFinish(clientID);
	}
	return(0);
}

